#include <ros/ros.h>
 #include <sensor_msgs/JointState.h>
 #include <trobot_kinematics/joint_states.h>
 #include <moveit/move_group_interface/move_group_interface.h>     
 #include <std_msgs/Int32.h>

class TrajectoryController
{
    #define JOINTS_NUM  trobot_kinematics::joint_states::JOINTS_NUM
public:

    explicit TrajectoryController(std::string robot);

    /**
     * @brief 控制机械臂移动到目标点
     * @param target 目标点
     * @note 目标是各个关节的最终位置
     */
    void MoveToTarget(std::vector<double>& target);

    /**
     * @brief 控制机械臂移动到目标点
     * @param target 目标点
     * @note 目标是末端姿态
     */
    void MoveToTarget(geometry_msgs::Pose& target);

    /**
     * @brief 主控制函数
     * @param ctrl_Hz 控制频率
     */
    void Control(void)
    {
        // 开启单线程 
        ros::AsyncSpinner spinner(1);
        spinner.start();       

        // 控制机械臂回到起点
       _arm.setNamedTarget("home");
       _arm.move();
       sleep(1);

        ROS_INFO("Successfully init joints Position ");
        sleep(2);

        std::vector<double> joint_group_positions(7);

        // 夹取点
        double targetPose1[7] = {0, -0.350599, 0.568743, 0.103399, 0.806408, 0.018448, 2.675177};
        for(int i = 0; i < JOINTS_NUM; i ++) joint_group_positions[i] = targetPose1[i]; 
        MoveToTarget(joint_group_positions);
        sleep(3);

        // 夹取
        std_msgs::Int32 msg;
        msg.data = 0xFF;
        gripper_puber.publish(msg);
        sleep(3);

        // 放置点
        double targetPose2[7] = {-3.14, -0.350599, 0.568743, 0.103399, 0.806408, 0.018448, 2.675177};
        for(int i = 0; i < JOINTS_NUM; i ++) joint_group_positions[i] = targetPose2[i]; 
        MoveToTarget(joint_group_positions);
         sleep(3);

        // 放置
        msg.data = 0xFE;
        gripper_puber.publish(msg);
        sleep(3);

        // // // 测试：B点
        // geometry_msgs::Pose target_pose;
        // target_pose.orientation.x = 1;
        // target_pose.orientation.y = 0;
        // target_pose.orientation.z = 0;
        // target_pose.orientation.w = 0;

        // target_pose.position.x = 0.2593;
        // target_pose.position.y = 0.0636;
        // target_pose.position.z = 0.1787;
        // MoveToTarget(target_pose);

        // 关闭线程 
        ros::shutdown(); 
    }
private:

    // 机器人名称
    std::string  _robot;

    ros::Publisher gripper_puber;
    ros::NodeHandle _node;

    // 路径规划组
    moveit::planning_interface::MoveGroupInterface _arm;
};
